
#include <stdio.h>

#include "led.h"
#include "usart.h"
#include "system.h"
#include "common.h"
#include "dynamixel.h"
#include "controller.h"
#include "motor_control.h"

#include "pwm.h"

#include <string.h>

const uint32_t systick_period_ms = 10;
volatile uint64_t local_time = 0;
volatile uint64_t local_time_old = 0;
volatile uint64_t local_time_old2 = 0;


template <USART_TEMPLATE>
class RemoteControl : public UsartInterrupt
{
  public:
    RemoteControl(void) {
      usart.init(this);
      power = 0;
      phase = 0;
      servo_speed = 0;
      current_speed = 0;
      current_power = 0;
    }

    void work() {
      usart.work();
    }
    
    void sendStatus() {
      static const int buffer_size = 60;
      static char s[buffer_size];
      snprintf(s, buffer_size, "  speed=%d\npower=%d\nangle=%d\n", current_speed, current_power, angle);
      int i = 0;
      while (s[i] != 0) {
        char a;
        a = s[i];
        usart.sendChar(a);
        //usart.work();
        i++;
      }
    }

    // Callback from USART
    void usartInterrupt(char data) {
      if (buffer.size() > BUFFER_SIZE - 2) {
        buffer.clear();
      }
      buffer += data;
  
      int row_length;
      while ((row_length = buffer.find('\n')) >= 0) {
        int split_position = buffer.find('=');
        if (split_position < row_length && 
            split_position > 0 &&
            row_length > split_position + 1) {
            //name  0 ... split_position
            //value split_position + 1 ...
              
          buffer[split_position] = 0;
          if(strcmp(buffer.c_str(), "servo_speed") == 0)
          {
            servo_speed = atoi(&buffer[split_position + 1]);
            servo_speed -= 1024;
            if (servo_speed < -1023) {
              servo_speed = -1023;
            } else if (servo_speed > 1023) {
              servo_speed = 1023;
            }
          }
  
          if(strcmp(buffer.c_str(), "power") == 0)
          {
            power = atoi(&buffer[split_position + 1]);
          }
          
          if(strcmp(buffer.c_str(), "phase") == 0)
          {
            phase = atoi(&buffer[split_position + 1]);
          }
          
          buffer.removehead(row_length);
        }
      }

    }
  
    void setCurrentSpeed(int current_speed) {
      this->current_speed = current_speed;
    }

    void setCurrentPower(int current_power) {
      this->current_power = current_power;
    }

    void setAngle(int angle) {
      this->angle = angle;
    }

    int getPower(void) {
      return power;
    }

    int getServoSpeed(void) {
      return servo_speed;
    }

    int getPhaseOffset(void) {
      return phase;
    }

  private:
    Usart<USART_TEMPLATE_PARAMS> usart;
    static const int BUFFER_SIZE = 100;
    String<BUFFER_SIZE> buffer;

    int current_speed;
    int current_power;
    int power;
    int servo_speed;
    int angle;
    int phase;
};

int main(void)
{
  //SystemInit();
  rccInit();
  nvicInit();
  //ioInit();
  //iwdgInit();

  /* SystTick configuration: an interrupt every 10ms */
  RCC_ClocksTypeDef RCC_Clocks;
  RCC_GetClocksFreq(&RCC_Clocks);
  SysTick_Config(RCC_Clocks.SYSCLK_Frequency / 100);

  /* Update the SysTick IRQ priority should be higher than the Ethernet IRQ */
  /* The Localtime should be updated during the Ethernet packets processing */
  NVIC_SetPriority (SysTick_IRQn, 1);  

  Dynamixel dynamixel;
  dynamixel.setSpeed(0);
  dynamixel.setTorqueEnable(true);
  RemoteControl<XBEE_USART> remote_control;
  MotorControl motor_control;
  motor_control.setDutyCycle(0);
  motor_control.setPhaseOffset(30);
  int previous_angle = 0;
  int tracked_angle = 0;
  float rpm = 0.0f;
  bool led_on = false;
  int servo_speed_old = 0;
  
  while (true) {
    int phase_offset = remote_control.getPhaseOffset();
    int power = remote_control.getPower();
    int servo_speed = remote_control.getServoSpeed();
    if (servo_speed != servo_speed_old) {
      dynamixel.setSpeed(servo_speed); 
      servo_speed_old = servo_speed;
    }
    motor_control.setPhaseOffset(phase_offset);
    

    //remote_control.setCurrentPower(power);

    if (power > 98) {
      power = 98;
    }
    motor_control.setDutyCycle(power);

    motor_control.updateAngleMeasurement(0);
    motor_control.updatePhase();
    remote_control.setAngle(motor_control.getAngle());

    int angle = motor_control.getAngle();

    if (local_time > local_time_old) {
      local_time_old = local_time;
      int delta_angle = 0;
      delta_angle = tracked_angle - previous_angle;
      if (delta_angle < 0) {
        delta_angle = -delta_angle;
      }
      //if (delta_angle > 180) {
      //  delta_angle = 360 - delta_angle;
      //}
    
      
      previous_angle = angle;
      tracked_angle = angle;
      rpm = delta_angle * 16.66666666666667f;
      remote_control.setCurrentSpeed(rpm);
   
    }

    remote_control.setCurrentPower(tracked_angle);

  {
      int delta_angle = 0;
      delta_angle = angle - ((tracked_angle+36000)%360); // Amount to update tracked angle
      if (delta_angle > 180) {
        tracked_angle -= 360 - delta_angle;
      } 
      else if (delta_angle < -180)
      {
        tracked_angle += (360 + delta_angle);
      }
      else
      {
        tracked_angle += delta_angle;
      }
  }

    

    
    if (local_time > local_time_old2 + 200) {
      local_time_old2 = local_time;
      remote_control.sendStatus();

      if (led_on) {
        led_on = false;
      } else {
        led_on = true;
      }
      dynamixel.setLed(led_on);
    }
      
    remote_control.work();
    dynamixel.work(local_time);
  }

  return 0;
}

extern "C" {
  void SysTick_Handler(void)
  {
    /* Update the LocalTime by adding SYSTEMTICK_PERIOD_MS each SysTick interrupt */
    local_time += systick_period_ms;
  }

  void HardFault_Handler(void)
  {
    // Loop until WDT reset
    while (1);
  }
}
